/*
 * HDLCProtocol.cpp
 *
 *  Created on: 5 nov. 2010
 *      Author: eric
 */

#include "HDLCProtocol.h"
#include "CircularQueue.h"

extern CircularQueue fromCanQueue;
extern CircularQueue toCanQueue;

void HDLCProtocol::sendByte(uint8_t byte) {
	if (byte == FRAME_BOUNDARY || byte == ESCAPE_OCTET) {
		Serial.write(ESCAPE_OCTET);
		Serial.write(byte ^ ESCAPEXOR_FLAG);
	} else
		Serial.write(byte);
}

void HDLCProtocol::startPacket() {
	outcrc.reset();
}
void HDLCProtocol::sendPreamble() {
	Serial.write(FRAME_BOUNDARY);
}

void HDLCProtocol::sendPostamble() {
	CRC crc = outcrc.get();
	sendByte(crc & 0xff);
	sendByte(crc >> 8);
	Serial.write(FRAME_BOUNDARY);
	Serial.flush();
}

void HDLCProtocol::sendData(const uint8_t * const buf, uint8_t size) {
	uint8_t i;
	for (i = 0; i < size; i++) {
		outcrc.update(buf[i]);
		sendByte(buf[i]);
	}
}

void HDLCProtocol::sendData(uint8_t c) {
	outcrc.update(c);
	sendByte(c);
}
uint8_t HDLCProtocol::processData(uint8_t bIn) {
	if (bIn == ESCAPE_OCTET) {
		unEscaping = true;
		return 0;
	}
	//digitalWrite(13, HIGH);

	if (bIn == FRAME_BOUNDARY && !unEscaping) {
		if (inPacket) {
			/* End of packet */
			if (pBufPtr) {
				/* 0 == false */
				/* pBufPtr != 0 so message*/

				processFrame();
				inPacket = false;
				return 1;
			}
		} else {
			/* Beginning of packet */
			pBufPtr = 0;
			inPacket = true;
			incrc.reset();
		}
	} else {
		if (unEscaping) {
			bIn ^= ESCAPEXOR_FLAG;
			unEscaping = false;
		}

		if (pBufPtr < maxPacketSize) {
			pBuf[pBufPtr++] = bIn;
		} else {
			/* Message too big */
		}
	}
	return 0;
}

void HDLCProtocol::setLinkDown() {
	linkUp = false;
}

void HDLCProtocol::setLinkUp() {
	linkUp = true;
}

void HDLCProtocol::startXMit() {
	// is there something to send
	if ((fromCanQueue.count() > 0) && (linkUp == false)) {
		setLinkUp();
		sendFrame(U_FRAME_SABM);
	}
}

void HDLCProtocol::sendFrame(uint8_t const command, Can::CAN_MSG canMsg) {
	startPacket();
	sendPreamble();
	lastControlByte = command;
	sendData(command);
	sendData((uint8_t *) &canMsg.ID, 4);
	sendData(canMsg.data, canMsg.nbytes);
	sendPostamble();
}

void HDLCProtocol::sendFrame(uint8_t const command) {
	startPacket();
	sendPreamble();
	lastControlByte = command;
	sendData(command);
	sendPostamble();
}

void HDLCProtocol::handleInformationFrame(uint8_t *pBuf) {
	if (pBufPtr - 3 > 0) {
		// create CAN message
		Can::CAN_MSG canMsg;
		Can::uint32_b4 canID;
		canID.bID[0] = pBuf[1];
		canID.bID[1] = pBuf[2];
		canID.bID[2] = pBuf[3];
		canID.bID[3] = pBuf[4];
		canMsg.ID = canID.uID;
		canMsg.nbytes = pBufPtr - 7;
		canMsg.rtr = false;
		for (byte i = 0; i < canMsg.nbytes; i++) {
			canMsg.data[i] = pBuf[5 + i];
		}
		// if error send RNR
		// send CAN message to MCP2515
		toCanQueue.push(canMsg);
		// send RR frame
		sendFrame(S_FRAME_RR);
	}
}

void HDLCProtocol::handleUnnumberedFrame(uint8_t *pBuf) {
	switch (pBuf[0]) {
	case U_FRAME_SABM:
		if (!linkUp) {
			if (toCanQueue.count() == 16)
				sendFrame(U_FRAME_DM);
			else if (fromCanQueue.count() == 16)
				sendFrame(U_FRAME_DM);
			else {
				setLinkUp();
				sendFrame(U_FRAME_UA);
			}
		} else {
			sendFrame(U_FRAME_DM);
		}
		break;
	case U_FRAME_DM:
		setLinkDown();
		break;
	case U_FRAME_UA:
		if (lastControlByte == U_FRAME_SABM) {
			if (fromCanQueue.count() > 0) {
				sendFrame(I_FRAME_DATA, fromCanQueue.peek());
			} else {
				sendFrame(U_FRAME_DISC);
			}
		} else {
			setLinkDown();
		}
		break;
	case U_FRAME_DISC:
		sendFrame(U_FRAME_UA);
		setLinkDown();
		break;
	default:
		sendFrame(U_FRAME_DISC);
		break;
	}
}

void HDLCProtocol::handleSupervisoryFrame(uint8_t *pBuf) {
	switch (pBuf[0]) {
	case S_FRAME_RR: // RR receive ready, packet have only one frame so send
		// DISC
		fromCanQueue.pop();
		sendFrame(U_FRAME_DISC);
		break;
	case S_FRAME_RNR: // RNR receive not ready
		sendFrame(U_FRAME_DISC);
		break;
	case S_FRAME_REJ: // REJ rejected
		sendFrame(I_FRAME_DATA, fromCanQueue.peek());
		break;
	default:
		sendFrame(U_FRAME_DISC);
		break;
	}
}
void HDLCProtocol::processFrame() {
	if (pBufPtr < 3) {
		// less than 3 bytes so wrong message
		return;
	}

	//digitalWrite(13, HIGH);
	//delay(1000);
	//digitalWrite(13, LOW);
	//delay(1000);
	//digitalWrite(13, HIGH);
	//delay(1000);
	//digitalWrite(13, LOW);

	// crc 2 last bytes
	uint8_t i;
	incrc.reset();
	for (i = 0; i < pBufPtr - 2; i++) {
		incrc.update(pBuf[i]);
	}
	//message's crc
	CRC pcrc = *((CRC *) &pBuf[pBufPtr - 2]);
	if (pcrc != incrc.get()) {
		// CRC error
		sendFrame(S_FRAME_RNR);
		return;
	}

	if ((pBuf[0] & I_FRAME_BITS) == 0x0) {
		// Information frame
		handleInformationFrame(pBuf);
	} else if ((pBuf[0] & S_FRAME_BITS) == 0x1) {
		// Supervisory frame
		handleSupervisoryFrame(pBuf);
	} else {
		// Unnnumbered fram
		handleUnnumberedFrame(pBuf);
	}
	// send back message

	pBufPtr = 0;
}

